#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     armTouch,       sensorTouch)
#pragma config(Sensor, S3,     grabberTouch,   sensorTouch)
#pragma config(Motor,  motorA,          ,              tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          ,              tmotorNormal, openLoop)
#pragma config(Motor,  motorC,          ,              tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motorB,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorC,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     elbow,         tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     harvesterlift, tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    leftservo,            tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_2,    rightservo,           tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an TETRIX robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void grabberUp()
{
  while( SensorValue[grabberTouch] == 0) {
      servo[leftservo] = 255;
      servo[rightservo] = 0;
  }
  servo[leftservo] = 127;
  servo[rightservo] = 127;
}

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  wait1Msec(20);
  grabberUp();
  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
//   1. Have the robot follow a line on the game field until it reaches one of the puck storage
//      areas.
//   2. Load pucks into the robot from the storage bin.
//   3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
  initializeRobot();

  waitForStart(); // Wait for the beginning of autonomous phase.

  //red inside


motor [harvesterlift]= 50;
  wait1Msec (200);
  motor [harvesterlift]=0;
  motor [motorB]=60;
  motor [motorC]=60;
  wait1Msec (1025);
  motor [motorB]= 30;
  motor [motorC]=30;
  wait1Msec (300);
  motor [motorB]=0;
  motor [motorC]=0;
  wait1Msec (100);
  grabberUp();
  wait1Msec (100);
  motor [motorC]=50;
  motor [motorB]=-50;
  wait1Msec (2550);
  motor [motorB]=0;
  motor [motorC]=0;
  wait1Msec (100);
  motor [motorB]=60;
  motor [motorC]=60;
  wait1Msec (3200);
  motor [motorB]=0;
  motor [motorC]=0;
  wait1Msec (100);
  grabberUp();
  wait1Msec (100);
  motor [motorC]=30;
  motor [motorB]=-30;
  wait1Msec (500);
  motor [motorB]=30;
  motor [motorC]=30;
  wait1Msec (200);
  motor [motorB]=0;
  motor [motorC]=0;




  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  ////                                                   ////
  ////    Add your robot specific autonomous code here.  ////
  ////                                                   ////
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////

  while (true)
  {}
}
